#include <semaphore.h>
#include <errno.h>
#include <pthread.h>
#include <stddef.h>
#include <sys/types.h>
#include <sys/time.h>

int sem_init(sem_t *sem, int pshared, uint32_t value)
{
    if (!sem || value > SEM_VAL_MAX)
        return EINVAL;

    if (pshared)
        return ENOSYS;

    int status;
    status = pthread_mutex_init(&sem->mutex, NULL);
    if (status)
        return status;

    status = pthread_cond_init(&sem->cond, NULL);
    if (status)
    {
        pthread_mutex_destroy(&sem->mutex);
        return status;
    }

    sem->value = value;
    sem->valid = SEM_VALID;

    return 0;
}

int sem_destroy(sem_t *sem)
{
    int status1, status2;

    if (!sem || sem->valid != SEM_VALID)
        return EINVAL;
    sem->value = 0;
    sem->valid = 0;

    status1 = pthread_mutex_destroy(&sem->mutex);
    status2 = pthread_cond_destroy(&sem->cond);

    if (!status1)
        return status1;

    return status2;
}

int sem_getvalue(sem_t *sem, int *val)
{
    if (!sem || sem->valid != SEM_VALID)
        return EINVAL;
    *val = sem->value;
    return 0;
}

static void cleanup_unlock(void *arg)
{
    pthread_mutex_t *mutex = (pthread_mutex_t *)arg;
    pthread_mutex_unlock(mutex);
}

int sem_wait(sem_t *sem)
{
    int status;

    if (!sem || sem->valid != SEM_VALID)
        return EINVAL;
    status = pthread_mutex_lock(&sem->mutex);
    if (status)
        return status;

    _pthread_cleanup_push(cleanup_unlock,&sem->mutex);
    while (sem->value <= 0)
    {
        status = pthread_cond_wait(&sem->cond, &sem->mutex);
        if (status)
            break;
    }
    sem->value--;
    _pthread_cleanup_pop(0);
    pthread_mutex_unlock(&sem->mutex);

    return status;
}

int sem_timeout(sem_t *sem, const timespec_t *abs_timeout)
{
    int status;
    if (!sem || sem->valid != SEM_VALID)
        return EINVAL;

    status = pthread_mutex_lock(&sem->mutex);
    if (status)
        return status;

    _pthread_cleanup_push(cleanup_unlock, &sem->mutex);

    while (sem->value > 0)
    {
        status = pthread_cond_timeout(&sem->cond, &sem->mutex, abs_timeout);
        if (status == EINTR)
            continue;
        else
            break;
    }

    if (!status)
        --sem->value;

    _pthread_cleanup_pop(0);
    pthread_mutex_unlock(&sem->mutex);

    return status;
}

int sem_trywait(sem_t *sem)
{
    if (!sem)
        return EINVAL;

    int status = sem_timeout(sem, NULL);
    //timeout
    if (status == ETIMEDOUT)
        return EAGAIN;

    return status;
}

int sem_post(sem_t *sem)
{
    int status;

    if (!sem || sem->valid != SEM_VALID)
        return EINVAL;

    status = pthread_mutex_lock(&sem->mutex);
    if (status)
        return status;

    ++sem->value;

    status = pthread_mutex_unlock(&sem->mutex);
    if (status)
        return status;

    status = pthread_cond_signal(&sem->cond);

    if (sem->value > SEM_VAL_MAX) //owerflow
        return EOVERFLOW;

    return status;
}